/*
Copyright (C) Ariff Ehsan, Abdul Hadi Fikri

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/
/*  Author : Adib
 *  Date   : June 24, 2009
 */
 
#include <system.h>


void MOTOR_init(void) {

	/* SET pin PB5, PB6, PB7 and PD6 as the motor control pins */
	DDRD |= (1<<PD6);
	DDRB |= (1<<PB5)|(1<<PB6)|(1<<PB7);
	
	/* SET pin PD4 (OC1B), PD5 (OC1A) as the PWM output pinst*/
	DDRD |= (1<<PD4)|(1<<PD5);
	
	/* configure left motor to stop mode for initial condition */
	SET_BIT(PORTD, PD6);
	SET_BIT(PORTB, PB5);
	
	/* configure right motor to stop mode for initial condition */
	SET_BIT(PORTB, PB6);
	SET_BIT(PORTB, PB7);
	
	/* Enable pin PD4 and pin PD5 as the PWM output pins.
	   Set the hardware timer mode to phase correct-PWM 10-bit mode. */
	TCCR1A = (1<<COM1A1)|(1<<COM1B1)|(1<<WGM11)|(1<<WGM10);
	
	/* set 0% duty cycle */
	OCR1A = 0x00;
	OCR1B = 0x00;
	
	/* set prescaler */
	TCCR1B = (1<<CS11)|(1<<CS10);
	
}	//end MOTOR_init function

void MOTOR_setSpeed(uint8_t motor, uint16_t speed){
	
	speed &= 0x3ff;
	
	//speed left - OCR1B(PD4), speed right - OCR1A(PD5)
	
	if (motor == MOTOR_LEFT)
		OCR1B = speed;
	else
		OCR1A = speed;	

}	//end MOTOR_setSpeed function

void MOTOR_setDir(uint8_t motor, uint8_t dir){

	if (motor == MOTOR_RIGHT) {		//left motor dir control - PD6, PB5
		
		if (dir == MOTOR_FORWARD) {
			CLR_BIT(PORTD, PD6);
			SET_BIT(PORTB, PB5);
		}
		else if (dir==MOTOR_BACKWARD) {
			SET_BIT(PORTD, PD6);
			CLR_BIT(PORTB, PB5);		
		}
		else {
			CLR_BIT(PORTD, PD6);
			CLR_BIT(PORTB, PB5);		
		}
		
	}
	
	else {						//right motor dir control - PB6, PB7
	
		if(dir==MOTOR_BACKWARD) {
			CLR_BIT(PORTB, PB6);
			SET_BIT(PORTB, PB7);
		}
		else if(dir==MOTOR_FORWARD) {
			SET_BIT(PORTB, PB6);
			CLR_BIT(PORTB, PB7);		
		}	
		else {
			CLR_BIT(PORTB, PB6);
			CLR_BIT(PORTB, PB7);		
		}
	
	}
	
}	//end MOTOR_setDir function

